/*!
 * Copyright (c) 2012
 * Automation and Robotics Lab (LARA) at University of Brasilia (UnB)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     - Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     - Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     - Neither the name of the Automation and Robotics Lab (LARA) nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 *****************************************************************
 *
 *   ROS stack name: schunk
 *   ROS package name: schunk_joy
 *
 *   Authors: André A. Geraldes, email: andregeraldes@lara.unb.br
 *            Murilo M. Marinho, email: murilomarinho@lara.unb.br
 *
 *****************************************************************/

#ifndef SCHUNK_JOY_HEADER_GUARD
#define SCHUNK_JOY_HEADER_GUARD

/******************************************************************************
 		                          INCLUDES
 *****************************************************************************/

// ROS
#include <ros/ros.h>
#include <ros/callback_queue.h>

// Messages
#include <sensor_msgs/Joy.h>
#include <schunk_msgs/JointVelocities.h>
#include <schunk_msgs/CartesianPositions.h>
#include <schunk_msgs/DualQuaternion.h>
#include <schunk_msgs/CartesianVelocities.h>
#include <schunk_high/Init.h>
#include <schunk_high/Enable.h>
#include <schunk_high/Disable.h>
#include <schunk_low/GripperSetVel.h>
#include <schunk_low/GripperOpen.h>

/******************************************************************************
 		                   CLASS SCHUNK_LOW_CONTROL
 *****************************************************************************/

// Enumerations
enum Controllers {NO_CONTROLLER = 0, JOINT_CONTROLLER = 1, POSITION_CONTROLLER = 2, POSE_CONTROLLER = 3, VELOCITY_CONTROLLER = 4};

class SchunkJoystick
{

private:

	// Node Handlers
	ros::NodeHandle service_client_nodehandle_;
	ros::NodeHandle publisher_nodehandle_;
	ros::NodeHandle joy_nodehandle_;
	ros::NodeHandle position_nodehandle_;

	// Callback Queues
	ros::CallbackQueue service_client_queue_;
	ros::CallbackQueue publisher_queue_;
	ros::CallbackQueue position_queue_;

	// Publishers
	ros::Publisher publisher_joint_;

	// Subscribers
	ros::Subscriber subscriber_joy_references_;
	ros::Subscriber subscriber_end_effector_position_;

	// Service Servers
	ros::ServiceClient service_client_joint_init_control_;
	ros::ServiceClient service_client_joint_enable_control_;
	ros::ServiceClient service_client_joint_disable_control_;

	ros::ServiceClient service_client_gripper_set_vel_;
	ros::ServiceClient service_client_gripper_open_;

	// Messages
	schunk_msgs::JointVelocities message_joint_;
	schunk_low::GripperSetVel service_grip_vel_message_;
	schunk_high::Enable service_enable_message_;
	schunk_high::Init service_init_message_;

	// Button Variables
	int button_dead_man_;
	int button_init_;
	int button_grip_open_;
	int button_grip_close_;

	int button_control_set_0_;
	int button_control_set_1_;
	int button_control_set_2_;

	// Velocity Variables
	double axes_threshold_;
	double max_velocity_;
	double gripper_velocity_;

	// Position Variables
	double initial_x_;
	double initial_y_;
	double initial_z_;

	// Status Variables
	bool connected_;
	bool moving_gripper_;
	bool moving_robot_;
	int selected_control_set_;
	Controllers selected_controller_;

	bool joint_controller_available_;

	bool initial_position_read_;

	// Subscribed Topic Callbacks
	void JoyCallback(const sensor_msgs::Joy::ConstPtr& msg);

	// Auxiliar functions
	void CheckJoystickAxesAndMoveRobot(const sensor_msgs::Joy::ConstPtr& msg);

	// Inline functions
	inline bool ButtonSwitchControlSetPressed(const sensor_msgs::Joy::ConstPtr& msg);
	inline void SendZeroJointVelocitiesToTheRobot();

public:

	// Node Handlers
	ros::NodeHandle nodehandle_;

	// Callback Queues
	ros::CallbackQueue joy_queue_;

	// Constructor
	SchunkJoystick();

	// Destructor
	~SchunkJoystick();

	// Parameter loading functions
	bool GetButtonConfiguration();
};

#endif // SCHUNK_JOY_HEADER_GUARD
